#include "stlrender.h"
#include "ui_stlrender.h"
#include <QDebug>
#include<iostream>
#include <QTime>
using namespace  std;
void stlrender::OnSerialData(ProtoData dat)
{
    qDebug()<<"stlrender receive data"<<dat.funcode<<dat.len;
    GlobalVarManager::Instance()->SerialProto().WriteData(dat.funcode,(uint8_t *)dat.data,6);

}

stlrender::stlrender(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::stlrender),
    mLink1(0,230.95,-PI/2,0),
    mLink2(220,0,0,-PI/2),
    mLink3(0,0,PI/2,-PI/2),
    mLink4(0,-246.06,-PI/2,-PI/3),
    mLink5(0,0,PI/2,-PI/4),
    mLink6(0,-168.27,-PI/2,0),
    mRobot(6)
{
    ui->setupUi(this);
    this->setWindowTitle(QString("MOVEO 仿真软件"));
    setWindowIcon(QIcon("机器.png"));
    mTimerUpdate.setInterval(100);
    mTimerUpdate.start();
    mClock = clock();
    mPassTime = 0;
    connect(&mTimerUpdate, SIGNAL(timeout()), this, SLOT(go()));

    ui->lineEdit->setText("0.0");
    ui->lineEdit_2->setText("0.0");
    ui->lineEdit_3->setText("0.0");
    ui->lineEdit_4->setText("0.0");
    ui->lineEdit_5->setText("0.0");
    ui->lineEdit_6->setText("0.0");

    mRobot.AppendLink(mLink1);
    mRobot.AppendLink(mLink2);
    mRobot.AppendLink(mLink3);
    mRobot.AppendLink(mLink4);
    mRobot.AppendLink(mLink5);
    mRobot.AppendLink(mLink6);

    connect(this->mAdjustDiag,SIGNAL(on_change_x_position(uint16_t ,float )),
       ui->widget,SLOT(on_change_x_position(uint16_t ,float )));
    connect(this->mAdjustDiag,SIGNAL(on_change_y_position(uint16_t ,float )),
       ui->widget,SLOT(on_change_y_position(uint16_t ,float )));
    connect(this->mAdjustDiag,SIGNAL(on_change_z_position(uint16_t ,float )),
       ui->widget,SLOT(on_change_z_position(uint16_t ,float )));

    connect(&GlobalVarManager::Instance()->SerialProto(),SIGNAL(OnPackage(ProtoData)),this,
            SLOT(OnSerialData(ProtoData)));
    connect(&mTimer,SIGNAL(timeout()),this,SLOT(timerout()));
    mTimer.start(1000);
    mTimer.setSingleShot(true);
    this->_updateJointStatus();
}

stlrender::~stlrender()
{
    this->mAdjustDiag->close();
    delete ui;
}

void stlrender::go()
{
    dt = 1.0 * (clock() - mClock) / CLOCKS_PER_SEC;
    mClock = clock();
    mPassTime += dt;
    ui->label_joint1->setText(QString("%1").arg(ui->widget->mLink1.Theta()/2/PI*360));
    ui->label_joint2->setText(QString("%1").arg(ui->widget->mLink2.Theta()/2/PI*360));
    ui->label_joint3->setText(QString("%1").arg(ui->widget->mLink3.Theta()/2/PI*360));
    ui->label_joint4->setText(QString("%1").arg(ui->widget->mLink4.Theta()/2/PI*360));
    ui->label_joint5->setText(QString("%1").arg(ui->widget->mLink5.Theta()/2/PI*360));
//    ui->label_joint6->setText(QString("%1").arg(ui->widget->mLink6.Theta()/2/PI*360));
    ui->widget->updateGL();
}


void stlrender::on_pushButton_clicked()
{
    this->mAdjustDiag->show();
}

void stlrender::on_pushButton_2_clicked()
{
    theta1 = ui->lineEdit->text().toFloat();
    theta2 = ui->lineEdit_2->text().toFloat();
    theta3 = ui->lineEdit_3->text().toFloat();
    theta4 = ui->lineEdit_4->text().toFloat();
    theta5 = ui->lineEdit_5->text().toFloat();
    theta6 = ui->lineEdit_6->text().toFloat();

    ui->widget->OnThetaChangeAngle(theta1,theta2,theta3,
                                   theta4,theta5,theta6);

    _updateJointStatus();

    ui->lineEdit->setText("0.0");
    ui->lineEdit_2->setText("0.0");
    ui->lineEdit_3->setText("0.0");
    ui->lineEdit_4->setText("0.0");
    ui->lineEdit_5->setText("0.0");
    ui->lineEdit_6->setText("0.0");

    ui->widget->InverseKinemathic();
}

void stlrender::on_pushButton_4_clicked()
{
    GlobalVarManager::Instance()->SeirialDialog().show();
}


void stlrender::on_pushButton_3_clicked()
{
    mTestDriverDialog->show();
}

void stlrender::_updateJointStatus()
{
    ui->label_joint1->setText(QString("%1").arg(ui->widget->mLink1.Theta()/2/PI*360));
    ui->label_joint2->setText(QString("%1").arg(ui->widget->mLink2.Theta()/2/PI*360));
    ui->label_joint3->setText(QString("%1").arg(ui->widget->mLink3.Theta()/2/PI*360));
    ui->label_joint4->setText(QString("%1").arg(ui->widget->mLink4.Theta()/2/PI*360));
    ui->label_joint5->setText(QString("%1").arg(ui->widget->mLink5.Theta()/2/PI*360));
//    ui->label_joint6->setText(QString("%1").arg(ui->widget->mLink6.Theta()/2/PI*360));

    ui->label_1_pos->setText(QString("(%1,%2,%3)").arg(0).arg(0).arg(0));
    ui->label_2_pos->setText(QString("(%1,%2,%3)").arg(ui->widget->mLink1.PosX()).arg(ui->widget->mLink1.PosY()).arg(ui->widget->mLink1.PosZ()));
    ui->label_3_pos->setText(QString("(%1,%2,%3)").arg(ui->widget->mLink2.PosX()).arg(ui->widget->mLink2.PosY()).arg(ui->widget->mLink2.PosZ()));
    ui->label_4_pos->setText(QString("(%1,%2,%3)").arg(ui->widget->mLink3.PosX()).arg(ui->widget->mLink3.PosY()).arg(ui->widget->mLink3.PosZ()));
    ui->label_5_pos->setText(QString("(%1,%2,%3)").arg(ui->widget->mLink4.PosX()).arg(ui->widget->mLink4.PosY()).arg(ui->widget->mLink4.PosZ()));
    ui->label_6_pos->setText(QString("(%1,%2,%3)").arg(ui->widget->mLink5.PosX()).arg(ui->widget->mLink5.PosY()).arg(ui->widget->mLink5.PosZ()));

    ui->lineEdit_pos_x->setText(QString("%1").arg(ui->widget->mLink5.PosX()));
    ui->lineEdit_pos_y->setText(QString("%1").arg(ui->widget->mLink5.PosY()));
    ui->lineEdit_pos_z->setText(QString("%1").arg(ui->widget->mLink5.PosZ()));
}


void stlrender::on_pushButton_5_clicked()
{
    ui->widget->InverseKinemathic();
    QList<double> dat = ui->widget->InverseTheta();
    QString label = "逆解:\r\n";
    for (uint16_t i = 0;i < dat.length();i++){
        label += QString("θ%1: %2 ").arg(i + 1).arg(dat[i]);
    }
    ui->label_32->setText(label);
}

void stlrender::timerout()
{
    _updateJointStatus();

    qDebug()<<"timeout";
}
